from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, TextSubstitution, PythonExpression
from launch_ros.actions import Node


def generate_launch_description():
    # 声明参数
    vehicle_id_arg = DeclareLaunchArgument(
        'vehicle_id',
        default_value="0",  # 起始的 vehicle_id
        description='Base Vehicle ID.'
    )
    namesspace_arg = DeclareLaunchArgument(
        'namespace',
        default_value="/drone",
        description='Namespace of the nodes.'
    )


    # 创建节点列表
    nodes = []


    # 循环创建多个节点
    for i in range(8):  # 创建 8 个节点
        # 计算动态的 vehicle_id 和 namespace

        # 创建节点
        node = Node(
            package="udp_gps_forwarder",
            executable="udp_gps_forwarder_node",
            name=f"udp_gps_forwarder_node_{i}",  # 节点名称
            output="screen",
            emulate_tty=True,
            parameters=[
                {"vehicle_id": i},  # 车辆 ID 参数
                {"namespace": 'namespace'},  # 命名空间参数
            ]
        )
        nodes.append(node)

    # 返回 LaunchDescription，包括声明的参数和所有节点
    return LaunchDescription([
        vehicle_id_arg,
        namesspace_arg,
        *nodes  # 解包节点列表
    ])
